#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/u_int8.hpp"
#include "std_msgs/msg/u_int16_multi_array.hpp"
#include "std_msgs/msg/int16_multi_array.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "geometry_msgs/msg/twist.hpp"

#include "ubt_com/ubt_com_util.h"
#include "ubt_interface/msg/ubt_com_defines.hpp"
#include "ubt_state/ubt_state_task_nav2.h"
#include "ubt_state/ubt_state_util.h"
#include "ubt_state/node.h"
namespace ubt_state
{
struct NavClsData2
{
    float _obstacle_stop_dis;
    float _obstacle_slow_dis;
    float _obstacle_warn_dis;
    float _obstacle_side_dis;
    float _obstacle_run_start_up_dis;
    ObstacleParams obsParams;

    rclcpp::Publisher<ubt_interface::msg::UbtComMsg>::SharedPtr _pub_nav_in;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr _pub_real_route_info;
    rclcpp::Publisher<ubt_interface::msg::NavPathInfo>::SharedPtr _pub_nav_path_info;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pub_target_point_name;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pub_target_point_id;
    rclcpp::Publisher<ubt_interface::msg::NavTaskLimit>::SharedPtr _pub_nav_task_limit;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr _pub_nav_task_control;
    rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr _pub_nav_speed_limit_qr;
    rclcpp::Publisher<geometry_msgs::msg::Pose2D>::SharedPtr _pub_current_pose;
    rclcpp::Publisher<geometry_msgs::msg::Pose2D>::SharedPtr _pub_current_rfid;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pub_current_name;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pub_current_id;
    rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr _pub_current_index;
    rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr _pub_current_QRIndex;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pub_end_rfid_broken;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr _sub_report_qr_info;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr _sub_nav_task_result;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr _sub_control_type;
    rclcpp::Subscription<ubt_interface::msg::UbtComMsg>::SharedPtr _sub_nav_out;
    rclcpp::Subscription<ubt_interface::msg::Rfid>::SharedPtr _sub_nav_rfid;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr _sub_cmd_vel;//矢量速度
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr _pub_smooth_vel;//矢量平滑速度
    

    CTaskNav2 *_pCurTask;
    short _nextMsgId;
    std::string navType;
};
static NavClsData2 g_clsData;
static std::string  current_rfid = "";//当前rfid
static int _last_task_id = -10000;//上一次任务的终点，T1000不能转向，只能补发


static void _PubObstacleDis(float stopDis, float slowDis, float warnDis, float sideDis)
{
    ObstacleParams obsParams;
    obsParams.stopDis = stopDis;
    obsParams.slowDis = slowDis;
    obsParams.warnDis = warnDis;
    obsParams.sideDis = sideDis;
    UbtStateUtilSetObstacleParams(obsParams);
}

static int _Line2Nav(ubt_interface::msg::Line& navLine, Line *pLine)
{
    int iRes = 0;
    std::vector<Point>::iterator iterPt;
    ubt_interface::msg::Point navPoint;
    navLine.type = pLine->type;
    for (iterPt = pLine->points.begin(); iterPt != pLine->points.end(); ++iterPt)
    {
        navPoint.id = iterPt->id;
        navPoint.x = iterPt->x;
        navPoint.y = iterPt->y;
        navPoint.t = iterPt->t;
        navPoint.avoid_dis = iterPt->avoidDis;
        navPoint.rfid = iterPt->rfid;
        navPoint.name = iterPt->name;
        navPoint.direction_index = iterPt->direction_index;
        navLine.points.push_back(navPoint);
    }
    return iRes;
}

static int _SendNavMsg(int msgType, std::string body)
{
    ubt_interface::msg::UbtComMsg msg;
    msg.proto_id = ubt_interface::msg::UbtComDefines::PROTO_ID_NAV;
    msg.msg_type = msgType;
    msg.msg_id = g_clsData._nextMsgId;
    g_clsData._nextMsgId++;
    msg.msg_time = rclcpp::Clock().now();
    msg.body = body;
    g_clsData._pub_nav_in->publish(msg);
    RCLCPP_INFO(node->get_logger(),"msgId=%d msgType=%d body=%s",
             msg.msg_id, msg.msg_type, msg.body.c_str());
    return 0;
}

static void _OnRecvNavRfidWrap(const ubt_interface::msg::Rfid::SharedPtr msg)
{
    if(g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->OnRecvNavRfidWrap(msg);
    }
}
void CTaskNav2::OnRecvNavRfidWrap(ubt_interface::msg::Rfid::SharedPtr  msg)
{
    if(_agv_type == "t1000_v2")
    {
        if(current_rfid == msg->id || msg->is_induction == 0) // 保证扫描到rfid只上报一次
        {
            return;
        }
        if(msg->id != "" && msg->is_induction == 1 )//扫描到有效的rfid
        {
            ubt_state::ulogger->info("扫描到rfid点--rfid={0:s}",msg->id );
            current_rfid = msg->id;   // 获取第一次有效的rfid，给_OnArrive函数的当前点补发0使用
        }
        std::vector<Line>::iterator iterLine;
        if(_wait_rfid == 1)//  _wait_rfid:rfid上报等待下发标志位，先等导航中间件处发-1，才发0,1,2
        {
            return;
        }
        
        RCLCPP_INFO(node->get_logger(),"pub points");
        bool in_current_path = false;//扫描到的rfid点，是否在当前下发的路径中
        
        for (iterLine = _pLineInfos->begin(); iterLine != _pLineInfos->end(); ++iterLine)
        {
            ubt_interface::msg::Line navLine;
            int iRes = _Line2Nav(navLine, &*iterLine);
            if (iRes != 0)
            {
                break;
            }
            int index = std::distance(_pLineInfos->begin(), iterLine);
            if(index ==_pLineInfos->size()-1 && navLine.points[1].rfid == msg->id)//最后一段路径，也是最后一个点
            {              
                if(_current_pose_type == "mag" )
                {
                    geometry_msgs::msg::Pose2D curPose;
                    curPose.x = navLine.points[1].x;
                    curPose.y = navLine.points[1].y;
                    curPose.theta = navLine.points[1].t;

                    g_clsData._pub_current_pose->publish(curPose);
                    rclcpp::sleep_for(std::chrono::milliseconds(100));
                    g_clsData._pub_current_pose->publish(curPose);
                    rclcpp::sleep_for(std::chrono::milliseconds(100));
                    g_clsData._pub_current_pose->publish(curPose);
                }

                std_msgs::msg::String id_msg;
                id_msg.data = std::to_string(navLine.points[1].id);
                g_clsData._pub_current_id->publish(id_msg);

                id_msg.data = navLine.points[1].name;
                g_clsData._pub_current_name->publish(id_msg);

                index = _pLineInfos->size();//上报最后一个点，index再置回来用于上报给导航中间件
                OnSiteChanged(index);
                in_current_path = true;

            }
            else if(navLine.points[0].rfid == msg->id)//到点就设置避障参数,并上报点位,只有变更rfid才做操作
            {
                _PubObstacleDis(navLine.points[0].avoid_dis, g_clsData._obstacle_slow_dis, g_clsData._obstacle_warn_dis, g_clsData._obstacle_side_dis);
                if(_current_pose_type == "mag" )
                {
                    geometry_msgs::msg::Pose2D curPose;
                    curPose.x = navLine.points[0].x;
                    curPose.y = navLine.points[0].y;
                    curPose.theta = navLine.points[0].t;
                    g_clsData._pub_current_pose->publish(curPose);
                    rclcpp::sleep_for(std::chrono::milliseconds(100));
                    g_clsData._pub_current_pose->publish(curPose);
                    rclcpp::sleep_for(std::chrono::milliseconds(100));
                    g_clsData._pub_current_pose->publish(curPose);
                }

                std_msgs::msg::String id_msg;
                id_msg.data = std::to_string(navLine.points[0].id);
                g_clsData._pub_current_id->publish(id_msg);

                id_msg.data = navLine.points[0].name;
                g_clsData._pub_current_name->publish(id_msg);

                OnSiteChanged(index);
                //告诉导航中间件当前index,否则会重复上报点位---融合用
                if(_current_pose_type == "fusion")
                {
                    std_msgs::msg::UInt8 QR_msg;
                    QR_msg.data = index;
                    g_clsData._pub_current_QRIndex->publish(QR_msg);
                }
                in_current_path = true;
            }
        }

        if(_point_status[_pLineInfos->size()] == 0 && in_current_path == false && _current_pose_type == "mag")//终点rfid坏了的情况(扫描到rfid但没上报,且终点也未上报,只有跑磁条才做此操作)，此时识别为终点的下一个rfid点，报结束，防止转圈现象
        {
            ubt_state::ulogger->info("==================== rfid终点坏了,报可恢复故障:{} ====================",msg->id);
            in_current_path = true;
            std_msgs::msg::String pub_msg;
            pub_msg.data = "1";
            g_clsData._pub_end_rfid_broken->publish(pub_msg); 
        }
        
    }

}
static void _OnRecvNavMsgWrap(const ubt_interface::msg::UbtComMsg::SharedPtr msg)
{
    int iRes;
    void *pvMsg;
    ubt_proto_nav::ubt_nav_msg_com_rsp  comRsp;
    ubt_proto_nav::ubt_nav_status  navStatus;
    do
    {
        if (msg->proto_id != ubt_interface::msg::UbtComDefines::PROTO_ID_NAV)
        {
            RCLCPP_WARN(node->get_logger(),"bad protoId msg->protoId=%d ", msg->proto_id);
            break;
        }
        
        RCLCPP_INFO(node->get_logger()," msgId=%d msgType=%d body=%s",
                 msg->msg_id, msg->msg_type, msg->body.c_str());
        std::string body = msg->body;
        std::string failDesc;
        if ((msg->msg_type == ubt_interface::msg::UbtComDefines::MSG_TYPE_STATUS_NTY) ||
                (msg->msg_type == ubt_interface::msg::UbtComDefines::MSG_TYPE_START_RSP))
        {
            ubt_proto_nav::ubt_nav_status  navStatus;
            iRes = ubt_proto_nav::ubtNavDecodeStatus(body, &navStatus, failDesc);
            if (iRes != 0)
            {
                RCLCPP_WARN(node->get_logger(),"decode navStatus fail iRes=%d msgType=%d",
                         iRes, msg->msg_type);
                break;
            }
            pvMsg = &navStatus;
        }
        else
        {
            iRes = ubt_proto_nav::ubtNavDecodeComRsp(body, &comRsp, failDesc);
            if (iRes != 0)
            {
                RCLCPP_WARN(node->get_logger(),"decode comRsp fail iRes=%d msgType=%d",
                         iRes, msg->msg_type);
                break;
            }
            pvMsg = &comRsp;
        }
        if (g_clsData._pCurTask != NULL)
        {
            g_clsData._pCurTask->OnRecvNavMsg(*msg, pvMsg);
            break;
        }
        if ((msg->msg_type == ubt_interface::msg::UbtComDefines::MSG_TYPE_STOP_RSP) ||
                ((msg->msg_type == ubt_interface::msg::UbtComDefines::MSG_TYPE_STATUS_NTY) && (navStatus.state <= 0)))
        {
            break;
        }
        //_SendStopReq(ubt_task_result_mod_timeout);
    }
    while (false);
    
}

static void _ReportQRInfoWrap(const std_msgs::msg::Int32::SharedPtr pathIndex)
{
    if(g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->OnSiteChanged(pathIndex->data);
    }
}
static void control_type_callback(const std_msgs::msg::Int32::SharedPtr msg)
{
    int result = msg->data;
    if(g_clsData._pCurTask != NULL)
    {
   	 g_clsData._pCurTask->controlType = result;
    }
}
static void _RecvNavTaskResult(const std_msgs::msg::Int32::SharedPtr navTaskResult)
{
    int result = navTaskResult->data;
    // 如果导航任务成功
    if(result == 0)
    {
        if(g_clsData._pCurTask != NULL)
        {
            g_clsData._pCurTask->SetStateWrap(ubt_task_state_closed, result, "nav task success");
        }
    }
}
//矢量速度--从全局变量来
static void _Cmd_Vel_callbackWrap(const geometry_msgs::msg::Twist::SharedPtr msg){
    if (g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->Cmd_Vel_callback(msg);
    }
}
//矢量速度
void CTaskNav2::Cmd_Vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
    current_speed      = msg->linear.x;
	current_angular    = msg->angular.z;
}
//矢量平滑速度--从全局变量来
static void _Smooth_Vel_callbackWrap(const geometry_msgs::msg::Twist::SharedPtr msg)
{
    if (g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->Smooth_Vel_callback(msg);
    }
}
//矢量平滑速度
void CTaskNav2::Smooth_Vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
    current_speed      = msg->linear.x;
	current_angular    = msg->angular.z;
}
int CTaskNav2::ClassInit(ObstacleParams obsParams)
{
    
    NavClsData2 *pClsData = &g_clsData;
    pClsData->_obstacle_stop_dis = obsParams.stopDis;
    pClsData->_obstacle_slow_dis = obsParams.slowDis;
    pClsData->_obstacle_warn_dis = obsParams.warnDis;
    pClsData->_obstacle_side_dis = obsParams.sideDis;
    RCLCPP_INFO(node->get_logger(),"_stop_dis=%f _slow_dis=%f _warn_dis=%f",
             pClsData->_obstacle_stop_dis, pClsData->_obstacle_slow_dis, pClsData->_obstacle_warn_dis);

    pClsData->_pub_nav_in = node->create_publisher<ubt_interface::msg::UbtComMsg>("/tbu_nav_info_in", 1);
    pClsData->_pub_real_route_info = node->create_publisher<std_msgs::msg::Int32MultiArray>("/real_route_info", 10);
    pClsData->_pub_nav_path_info = node->create_publisher<ubt_interface::msg::NavPathInfo>("/nav_path_info", 10);
    pClsData->_pub_target_point_name = node->create_publisher<std_msgs::msg::String>("/target_point_name", 1);
    pClsData->_pub_target_point_id = node->create_publisher<std_msgs::msg::String>("/target_point_id", 1);
    pClsData->_pub_nav_task_limit = node->create_publisher<ubt_interface::msg::NavTaskLimit>("/nav_task_limit", 1);
    // 导航任务控制，0:stop, 2:pause, 3:continue
    pClsData->_pub_nav_task_control = node->create_publisher<std_msgs::msg::Int32>("/nav_task_control", 2);
    // 二维码遇到障碍物后，给tbu_mag_navigate_middle功能包发送减速或者停止
    pClsData->_pub_nav_speed_limit_qr = node->create_publisher<std_msgs::msg::UInt8>("/nav_speed_limit_qr", 1);
    pClsData->_pub_current_pose = node->create_publisher<geometry_msgs::msg::Pose2D>("/current_pose", 1); //发布当前位置坐标
    pClsData->_pub_current_rfid = node->create_publisher<geometry_msgs::msg::Pose2D>("/current_test_rfid", 10); //测试用topic
    pClsData->_pub_current_id = node->create_publisher<std_msgs::msg::String>("/current_id", 10); //发布当前位置id
    pClsData->_pub_end_rfid_broken = node->create_publisher<std_msgs::msg::String>("/end_rfid_broken", 10); //rfid终点坏了
    pClsData->_pub_current_index = node->create_publisher<std_msgs::msg::UInt8>("/current_index_test", 10); //发布index-测试

    pClsData->_pub_current_QRIndex = node->create_publisher<std_msgs::msg::UInt8>("/current_QRIndex", 1); //发布当前路径中的index，给到导航中间件
    pClsData->_pub_current_name = node->create_publisher<std_msgs::msg::String>("/current_name", 10); //发布当前位置名称

    pClsData->_sub_nav_out = node->create_subscription<ubt_interface::msg::UbtComMsg>("/tbu_nav_info_out", 10, std::bind(&_OnRecvNavMsgWrap, std::placeholders::_1));
    // 上报机器人当前所在的二维码
    pClsData->_sub_report_qr_info = node->create_subscription<std_msgs::msg::Int32>("/report_qr_info", 10, std::bind(&_ReportQRInfoWrap, std::placeholders::_1));
    pClsData->_sub_nav_rfid = node->create_subscription<ubt_interface::msg::Rfid>("/rfid_state", 10, std::bind(&_OnRecvNavRfidWrap, std::placeholders::_1));
    // 订阅导航任务结果
    pClsData->_sub_nav_task_result = node->create_subscription<std_msgs::msg::Int32>("/nav_task_result", 10,std::bind(&_RecvNavTaskResult, std::placeholders::_1));
    pClsData->_sub_control_type = node->create_subscription<std_msgs::msg::Int32>("/control_type", 10,std::bind(&control_type_callback ,std::placeholders::_1));
    pClsData->_pCurTask = NULL;
    pClsData->_nextMsgId = 0;

    pClsData->_sub_cmd_vel = node->create_subscription<geometry_msgs::msg::Twist>("/instance_cmd_vel", 1, std::bind(&_Cmd_Vel_callbackWrap, std::placeholders::_1));
    pClsData->_pub_smooth_vel = node->create_subscription<geometry_msgs::msg::Twist>("/smooth_cmd_vel", 1, std::bind(&_Smooth_Vel_callbackWrap, std::placeholders::_1));

    node->declare_parameter<float>("/obstacle_run_start_up_dis", float(0.15));
    pClsData->_obstacle_run_start_up_dis = node->get_parameter("/obstacle_run_start_up_dis").as_double();

    std::string  locAlg;
    node->declare_parameter("/agv_loc_algs", "");
     locAlg = node->get_parameter("/agv_loc_algs").as_string();
    // if(locAlg == "mag")
    // {
    //     pClsData->navType = "mag_nav";
    // }else if(locAlg == "amr" || locAlg == "qrcode" )
    // {
    //     pClsData->navType = "qr_nav";
    // }
    pClsData->navType = "qr_nav";

    
    return 0;
}

void CTaskNav2::ClassCleanup()
{
    NavClsData2 *pClsData = &g_clsData;
    // pClsData->_pub_nav_in->Shutdown();
    // pClsData->_pub_real_route_info->Shutdown();
    // pClsData->_pub_nav_path_info->Shutdown();
    // pClsData->_pub_target_point_id->Shutdown();
    // pClsData->_pub_target_point_name->Shutdown();
    // pClsData->_pub_nav_task_limit->Shutdown();
    // pClsData->_pub_nav_task_control->Shutdown();
    // pClsData->_pub_nav_speed_limit_qr->Shutdown();
    // pClsData->_sub_nav_out->Shutdown();
    // pClsData->_sub_report_qr_info->Shutdown();
    // pClsData->_sub_nav_task_result->Shutdown();
    pClsData->_pCurTask = NULL;
}

ITask *CTaskNav2::CreateTask(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *pLineInfos,
    ubt_task_stage stage, int prePathAvoidHwMask, int index, int reportIndex)
{
    CTaskNav2 *pTask = new CTaskNav2();
    int chargeExLoop = -1;
    if (!pTask->Init(pTaskCb, taskId, pLineInfos, stage, prePathAvoidHwMask, index, reportIndex, chargeExLoop))
    {
        delete pTask;
        pTask = NULL;
    }
    return (ITask *)pTask;
}

ITask *CTaskNav2::CreateTask(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *pLineInfos,
    ubt_task_stage stage, int prePathAvoidHwMask, int index, int reportIndex, int chargeExLoop)
{
    CTaskNav2 *pTask = new CTaskNav2();
    if (!pTask->Init(pTaskCb, taskId, pLineInfos, stage, prePathAvoidHwMask, index, reportIndex, chargeExLoop))
    {
        delete pTask;
        pTask = NULL;
    }
    return (ITask *)pTask;
}

CTaskNav2::~CTaskNav2()
{
    RCLCPP_INFO(node->get_logger(),"_taskId=%s _state=%d", _taskId.c_str(), _state);
    if ((_state != ubt_task_state_idle) && (_state != ubt_task_state_closed))
    {
        _SetState(ubt_task_state_closed, -1, "destroy task wrong");
    }
    delete [] _point_status;
}

bool CTaskNav2::Init(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *pLineInfos,
    ubt_task_stage stage, int prePathAvoidHwMask, int index, int reportIndex, int chargeExLoop)
{
    _pTaskCb = pTaskCb;
    _taskId = taskId;
    _prePathAvoidHwMask = prePathAvoidHwMask;
    _index = index;
    _reportIndex = reportIndex;
    _pLineInfos = pLineInfos;

    _point_status = new int[_pLineInfos->size()+1]();//初始化数组，并设0

    _stage = stage;

    _state = ubt_task_state_idle;
    _stateTime = ubt_com::GetNowInMsecs();
    _flag = 0;
    _waitPathIndex = -1000;
    _waitPointId = -1000;
    _waitCmd = -1;

    _lastStatusTime = 0;
    _lastStatusQueryTime = 0;

    _stopReason = 0;
    _stopReasonDesc = "";

    _obstacleState = 0;
    _avoidHwStatus = 0;

    _pathIndex = -3;
    _lastPtId = -1;
    _nextPtId = -1;

    _chargeExLoop = chargeExLoop;

    node->declare_parameter("current_pose_type", "slam");
     _current_pose_type = node->get_parameter("current_pose_type").as_string();
    node->declare_parameter("agv_type", "t1000_v2");
     _agv_type = node->get_parameter("agv_type").as_string();

    RCLCPP_INFO(node->get_logger(),"ok taskId=%s stage=%d", taskId.c_str(), stage);
    return true;
}


bool CTaskNav2::Start()
{
    RCLCPP_INFO(node->get_logger(),"_taskId= %s _state=%d", _taskId.c_str(), _state);
    ubt_state::ulogger->info("任务开始--\n_taskId= {} _state={0:d}", _taskId.c_str(), _state);
    _wait_rfid = 1;//rfid上报等待初始化
    _pathIndex = -3;
    if (g_clsData._pCurTask != NULL)
    {
        RCLCPP_WARN(node->get_logger(),"fail curTask is not null, taskId=%s ", _taskId.c_str());
        return false;
    }
    if (_state != ubt_task_state_idle)
    {
        RCLCPP_WARN(node->get_logger(),"fail state=%d is not idle, taskId=%s", _state, _taskId.c_str());
        return false;
    }
    _SetState(ubt_task_state_running, 0, "recv cmd start");
    // _OnArrive(-1);
    //_ApplyLineProp(-1);
    OnTimer();
    return true;
}

void CTaskNav2::Pause()
{
    ubt_state::ulogger->info("暂停--\n_taskId= {} _state={0:d}", _taskId.c_str(), _state);
    RCLCPP_INFO(node->get_logger(),"taskId= %s  _state=%d ", _taskId.c_str(), _state);
    if (_state == ubt_task_state_running)
    {
        _SetState(ubt_task_state_pause, 0, "recv cmd pause");
        if (_HasSendStart())
        {
            _SendPauseReq();
        }
    }
}

void CTaskNav2::Continue()
{
    ubt_state::ulogger->info("继续--\n_taskId= {} _state={0:d}", _taskId.c_str(), _state);
    RCLCPP_INFO(node->get_logger(),"taskId= %s _state=%d  ", _taskId.c_str(), _state);
    if (_state == ubt_task_state_pause)
    {
        _SetState(ubt_task_state_running, 0, "recv cmd continue");
        if (_HasSendStart())
        {
            _SendContinueReq();
        }
    }
}

void CTaskNav2::Stop(int reason)
{
    ubt_state::ulogger->info("停住--\n_taskId= {} _state={0:d}", _taskId.c_str(), _state);
    RCLCPP_INFO(node->get_logger(),"taskId= %s _state=%d _closeReason=%d reason=%d ",
             _taskId.c_str(), _state, _stopReason, reason);
    if ((_state == ubt_task_state_idle) ||
            (_state == ubt_task_state_closed))
    {
        return;
    }
    _SetState(ubt_task_state_stopping, reason, "recv cmd stop");
}

void CTaskNav2::OnTimer()
{
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    do
    {
        if ((_state == ubt_task_state_idle) || (_state == ubt_task_state_closed))
        {
            break;
        }

        long now = ubt_com::GetNowInMsecs();
        if (!_HasSendStart())
        {
            if (_state == ubt_task_state_running)
            {
                _OnAvoidStatusCb();
                _CheckObstacle(1);

                // if have obstacle, don't send path to tbu_mag_navigation_middle
                if (_obstacleState == 1)
                {
                    break;
                }

                if (0 == _SendStartReq())
                {
                    _flag |= 0x01;	//标记已发start指令
                    _lastStatusTime = now;	//设置查询状态时间
                }
            }
            else if (_state == ubt_task_state_stopping)
            {
                //直接进closed状态
                _SetState(ubt_task_state_closed, _stopReason, _stopReasonDesc);
            }
            else
            {
            }
            break;
        }

/*         if (pStatus->nav2CheckTimeout != 0)
        {
            if ((now - _lastStatusTime) >= 3000)
            {
                //3秒未收到状态信息，每间隔一秒查询
                if ((now - _lastStatusQueryTime) >= 1000)
                {
                    _lastStatusQueryTime = now;
                    _SendNavMsg(ubt_interface::msg::UbtComDefines::PROTO_ID_NAV::msg_type_query_req, "");
                }
            }
            if (_state < ubt_task_state_stopping)
            {
                //5.5秒未收到状态信息,发Stop指令
                if ((now - _lastStatusTime) >= 5500)
                {
                    _SetState(ubt_task_state_stopping, ubt_task_result_mod_timeout, " nav status timeout");
                }
            }
        } */

        if (_state == ubt_task_state_stopping)
        {
            if(current_speed <= 0.01 && current_angular <= 0.01)
            {
                _SetState(ubt_task_state_closed, _stopReason, _stopReasonDesc);
            }
        }
    }
    while (false);
}

void CTaskNav2::OnMsg(int msgType, void *pvMsg)
{
    do
    {
        if ((_state == ubt_task_state_idle) ||
                (_state == ubt_task_state_closed) ||
                (_state == ubt_task_state_stopping))
        {
            break;
        }

        if (msgType == ubt_msg_type_current_pose)
        {
            // _OnLocationCb();
            // _CheckObstacle(2);
            break;
        }

        if (msgType == ubt_msg_type_nav_limit)
        {
            NavTaskLimitIndex *pLimit = (NavTaskLimitIndex *)pvMsg;
            _waitPathIndex = pLimit->waitLineIndex;
            _waitPointId = pLimit->waitPointId;
            _waitCmd = pLimit->cmd;

            // if don't need to wait, _waitPathIndex = -1000
            // if don't add  this, sometimes will misreport traffic control to web
            if(1 == _waitCmd)
            {
                _waitPathIndex = -1000;
                _waitPointId = -1000;
            }

            if (_HasSendStart())
            {
                RCLCPP_INFO(node->get_logger(),"pLimit->waitPointId = %d",pLimit->waitPointId);
                _PubNavLimit(pLimit);
            }
            break;
        }
        if (msgType == ubt_msg_type_avoid_status)
        {
            _OnAvoidStatusCb();
            _CheckObstacle(1);
            break;
        }
    }
    while (false);
}

ubt_task_type CTaskNav2::GetType()
{
    return ubt_task_type_nav2;
}

ubt_task_state CTaskNav2::GetState()
{
    RCLCPP_INFO(node->get_logger(),"[CTaskNav2] GetState _lastPtId=%d,_waitPointId=%d,_pathIndex=%d,_waitPathIndex=%d",_lastPtId,_waitPointId,_pathIndex,_waitPathIndex);
    ubt_task_state state = _state;
    if (state == ubt_task_state_running)
    {
        if ((_lastPtId != -2) && (_waitPointId == _lastPtId) && (fabs(_waitPathIndex - _pathIndex) <= 1))
        {
            state = ubt_task_state_pause;
        }
        else if (_obstacleState == 1)
        {
            state = ubt_task_state_obstacle;
        }

        //如果是起点交通管制，状态直接变为暂停
        if(0 == _waitPathIndex && 0 == _waitCmd)
        {
            state = ubt_task_state_pause;
        }
    }
    return state;
}

ubt_task_stage CTaskNav2::GetStage()
{
    return _stage;
}

std::string CTaskNav2::GetActionType()
{
    return "nav";
}

void CTaskNav2::GetIndex(int& index, int& reportIndex)
{
    index = _index;
    reportIndex = _reportIndex;
}

void CTaskNav2::GetStopInfo(int& stopReason, std::string& stopReasonDesc)
{
    stopReason = _stopReason;
    stopReasonDesc = _stopReasonDesc;
}

void CTaskNav2::GetLogs(std::vector<TaskLogItem>& logs)
{
}

void CTaskNav2::OnRecvNavMsg(const ubt_interface::msg::UbtComMsg& msg, void *pvMsg)
{
    
    int iRes;
    do
    {
        if ((_state == ubt_task_state_idle) ||
                (_state == ubt_task_state_closed))
        {
            break;
        }
        if (msg.msg_type == ubt_interface::msg::UbtComDefines::MSG_TYPE_START_RSP)
        {
            ubt_proto_nav::ubt_nav_msg_com_rsp *pComRsp = (ubt_proto_nav::ubt_nav_msg_com_rsp *)pvMsg;
            if (pComRsp->result != ubt_interface::msg::UbtComDefines::RESULT_OK)
            {
                RCLCPP_WARN(node->get_logger(),"fail taskId=%s result=%d desc=%s",
                         _taskId.c_str(), pComRsp->result, pComRsp->desc.c_str());
                int reason;
                if (pComRsp->result == ubt_interface::msg::UbtComDefines::RESULT_BAD_REQUEST)
                {
                    reason = ubt_task_result_req_bad_param;
                }
                else if (pComRsp->result == ubt_interface::msg::UbtComDefines::RESULT_BAD_STATUS)
                {
                    reason = ubt_task_result_sys_bad_status;
                    _SendStopReq(ubt_task_result_sys_bad_status);
                }
                else
                {
                    reason = ubt_task_result_mod_timeout;
                }
                _SetState(ubt_task_state_closed, reason, "start fail" + pComRsp->desc);
            }
            break;
        }
        if ((msg.msg_type == ubt_interface::msg::UbtComDefines::MSG_TYPE_STATUS_NTY) ||
                (msg.msg_type == ubt_interface::msg::UbtComDefines::MSG_TYPE_QUERY_RSP))
        {
            ubt_proto_nav::ubt_nav_status *pNavStatus = (ubt_proto_nav::ubt_nav_status *)pvMsg;
            if (pNavStatus->taskId == "")
            {
                if ((msg.msg_type != ubt_interface::msg::UbtComDefines::MSG_TYPE_STATUS_NTY) ||
                        (pNavStatus->state >= 0) ||
                        (pNavStatus->stopReason <= 0))
                {
                    RCLCPP_WARN(node->get_logger(),"check  fail taskId=%s ,msg.taskId is empty, bad msgType=%d  state=%d stopReason=%d",
                             _taskId.c_str(), msg.msg_type, pNavStatus->state, pNavStatus->stopReason);
                    break;
                }
            }
            else
            {
                if (pNavStatus->taskId != _taskId)
                {
                    RCLCPP_WARN(node->get_logger(),"check taskId fail taskId=%s msg.taskId=%s",
                             _taskId.c_str(), pNavStatus->taskId.c_str());
                    break;
                }
            }
            _obstacleState = pNavStatus->obstacle;
            _lastStatusTime = ubt_com::GetNowInMsecs();
            if (pNavStatus->state > 0)
            {
                //线段或站点切换
                //_OnSiteChanged(pNavStatus);
                break;
            }
            //任务完成
            int reason = pNavStatus->stopReason;
            if (pNavStatus->state == 0)
            {
                reason = 0;
            }
            else
            {
                if (reason == 0)
                {
                    reason = ubt_task_result_sys_mapupdate;
                }
                else if (reason < 0)
                {
                    reason *= -1;
                }
                else if (reason == 100)
                {
                    reason = ubt_task_result_req_bad_param;
                }
                else if (reason == 110)
                {
                    reason = ubt_task_result_sys_posefail;
                }
                else if (reason == 120)
                {
                    reason = ubt_task_result_nav_trackfail;
                }
                else if (reason == 130)
                {
                    reason = ubt_task_result_nav_aborte3m;
                }
                else
                {
                    reason = ubt_task_result_nav_abort;
                }
            }
            _SetState(ubt_task_state_closed, reason, pNavStatus->stopReasonDesc);
            break;
        }
    }
    while (false);
    
}

void CTaskNav2::_ApplyLineProp(int pathIndex)
{
    //-3 结束,-1 出发，非负数-路网上第几段
    if ((pathIndex >= (int)_pLineInfos->size()) ||
            (pathIndex < -3))
    {
        return;
    }
    // std_msgs::msg::UInt16MultiArray pathMsg;
    Line *pLine;
    Point *pPt1, *pPt2;
    float stopDis, slowDis, warnDis, sideDis;

    if (pathIndex >= 0)
    {
        pLine = &(*_pLineInfos)[pathIndex];
        pPt1 = &pLine->points[0];//0 is first point
        pPt2 = &pLine->points[1];//1 is last point
        stopDis = pPt1->avoidDis;
    }
    else if (pathIndex == -2)  // 到点避障距离设置为0
    {
        stopDis = 0.0;
    }
    else if (pathIndex == -1) // 起步避障距离
    {
        // stopDis = pLine->points.size() > 0 ? pLine->points[0].avoidDis : g_clsData._obstacle_run_start_up_dis;
        stopDis = g_clsData._obstacle_run_start_up_dis;
    }
    else
    {
        stopDis = g_clsData._obstacle_stop_dis;
    }
    slowDis = g_clsData._obstacle_slow_dis;
    warnDis = g_clsData._obstacle_warn_dis;
    sideDis = g_clsData._obstacle_side_dis;

    // stopDis = g_clsData._obstacle_stop_dis;
    // slowDis = g_clsData._obstacle_slow_dis;
    // warnDis = g_clsData._obstacle_warn_dis;
    // sideDis = g_clsData._obstacle_side_dis;

    RCLCPP_INFO(node->get_logger(),"send obstacle_dis _taskId= %s pathIndex=%d dis=(%f,%f,%f,%f)",
             _taskId.c_str(), pathIndex, stopDis, slowDis, warnDis, sideDis);
    _PubObstacleDis(stopDis, slowDis, warnDis, sideDis);
}

void CTaskNav2::_CheckObstacle(int event)
{
    int oldObstcalState = _obstacleState;
    _obstacleState = _avoidHwStatus;

    if (_obstacleState != oldObstcalState)
    {
        RCLCPP_INFO(node->get_logger()," _taskId= %s _obstacleState=%d event=%d oldObstcalState=%d",
                 _taskId.c_str(), _obstacleState, event, oldObstcalState);
        std_msgs::msg::UInt8 msg;
        msg.data = _obstacleState;
        RCLCPP_INFO_STREAM(node->get_logger(),"send nav_speed_limit _taskId= " << _taskId.c_str() << "  msg: " << msg.data);
        g_clsData._pub_nav_speed_limit_qr->publish(msg);
        _pTaskCb->NotifyTaskStateAndStage(GetState(), _stage, 0, "");
    }
}

void CTaskNav2::_SetState(ubt_task_state newState, int reason, std::string reasonStr)
{
    
    RCLCPP_INFO(node->get_logger(),"newState=%d oldState=%d reason=%d reasonStr=%s",
             newState, _state, reason, reasonStr.c_str());

    // ubt_state::ulogger->info("CTaskNav2::_SetState: newState={}, oldState={}, reason={}, reasonStr={}",
    //          newState, _state, reason, reasonStr.c_str());

    int oldState = _state;
    if (_state == newState)
    {
        if ((newState != ubt_task_state_stopping) ||
                (reason >= _stopReason))
        {
            return;
        }
    }
    else
    {
        _state = newState;
        _stateTime = ubt_com::GetNowInMsecs();
    }
    if (_state == ubt_task_state_running)
    {
        if (oldState == ubt_task_state_idle)
        {
            g_clsData._pCurTask = this;
        }
    }
    else if (_state == ubt_task_state_stopping)
    {
        _stopReason = reason;
        _stopReasonDesc = reasonStr;
        if (_HasSendStart())
        {
            _SendStopReq(reason);
        }
    }
    else if (_state == ubt_task_state_closed)
    {
        /*
            如果 state 被设置ubt_task_state_stopping 时的reason 为：
            ubt_task_result_usr_cancel/ubt_task_result_usr_dispatchcancel/ubt_task_result_abnormal_end/
            ubt_task_result_usr_brakebtn/ubt_task_result_usr_stopbtn/ubt_task_result_sys_antc，
            _stopReason 不变
        */ 
        if (_stopReason != ubt_task_result_usr_cancel &&
            _stopReason != ubt_task_result_usr_dispatchcancel &&
            _stopReason != ubt_task_result_navigation_cancel &&
            _stopReason != ubt_task_result_usr_brakebtn &&
            _stopReason != ubt_task_result_usr_stopbtn &&
            _stopReason != ubt_task_result_sys_antc)
        {
            _stopReason = reason;
        }
  
        if (reasonStr == "")
        {
            _stopReasonDesc = UbtStateGetTaskResultDesc(reason);
        }
        else
        {
            _stopReasonDesc = reasonStr;
        }
        g_clsData._pCurTask = NULL;
        // _ApplyLineProp(-3);
        // if (reason == 0)
        // {
        //     _pTaskCb->NotifyTaskEvent(UBT_TASK_EVENT_ARRIVED_STR);
        // }
    }
    _pTaskCb->NotifyTaskStateAndStage(GetState(), GetStage(), reason, reasonStr);
    
}

bool CTaskNav2::_HasSendStart()
{
    if (_flag & 0x01)
    {
        return true;
    }
    return false;
}

int CTaskNav2::_SendStartReq()
{
    
    int iRes = 0;
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    ubt_interface::msg::NavPathInfo pathMsg;
    pathMsg.task_id = _taskId;
    pathMsg.nav_type = g_clsData.navType;
    std::vector<Line>::iterator iterLine;
    
    for (iterLine = _pLineInfos->begin(); iterLine != _pLineInfos->end(); ++iterLine)
    {
        ubt_interface::msg::Line navLine;
        iRes = _Line2Nav(navLine, &*iterLine);
        if (iRes != 0)
        {
            break;
        }

        //如果是空载，发给导航空载最大限速
        if (pStatus->liftState == lift_state_down || pStatus->dragState == drag_state_down )
        {
            navLine.limit.x = iterLine->limit.x;
            navLine.limit.w = iterLine->limit.w;
        }
        //否则，发给导航负载最大限速
        else
        {
            navLine.limit.x = iterLine->limit.lx;
            navLine.limit.w = iterLine->limit.lw;
        }

        for (ubt_interface::msg::Point &value :navLine.points)
        {
            value.x_vel = navLine.limit.x;
            value.w_vel = navLine.limit.w;
        }
        pathMsg.path.push_back(navLine);
    }
    if (iRes == 0)
    {
        g_clsData._pub_nav_path_info->publish(pathMsg);
        int pathSize = pathMsg.path.size();
        if(pathSize > 0)
        {
            ubt_interface::msg::Line end_navLine = pathMsg.path[pathSize - 1];
            std_msgs::msg::String point_name_id;
            
            point_name_id.data = std::to_string(end_navLine.points[1].id);
            g_clsData._pub_target_point_id->publish(point_name_id);//发布终点信息

            std_msgs::msg::String point_name_msg;
            point_name_msg.data = end_navLine.points[1].name;
            g_clsData._pub_target_point_name->publish(point_name_msg);
        }
        

        ubt_state::ulogger->info("--下发路径给导航中间件--task_id={0:s},path.size={1:d}",pathMsg.task_id,pathMsg.path.size());
        for(int i = 0;i<pathMsg.path.size();i++)
        {
            ubt_state::ulogger->info("\t[{0:d}-->{4:d}] rfid[{8:s}->{9:s}] 坐标为：({1:f},{2:f},{3:f}),({5:f},{6:f},{7:f})]"
            ,pathMsg.path[i].points[0].id,pathMsg.path[i].points[0].x,pathMsg.path[i].points[0].y,pathMsg.path[i].points[0].t
            ,pathMsg.path[i].points[1].id,pathMsg.path[i].points[1].x,pathMsg.path[i].points[1].y,pathMsg.path[i].points[1].t
            ,pathMsg.path[i].points[0].rfid,pathMsg.path[i].points[1].rfid);
        }

    }
    
    return iRes;
}

void CTaskNav2::_SendPauseReq()
{
    std_msgs::msg::Int32 controlInfo;
    controlInfo.data = 2;
    g_clsData._pub_nav_task_control->publish(controlInfo);
    RCLCPP_INFO(node->get_logger(),"==================== exec ====================");

}

void CTaskNav2::_SendContinueReq()
{
    std_msgs::msg::Int32 controlInfo;
    controlInfo.data = 3;
    g_clsData._pub_nav_task_control->publish(controlInfo);
    RCLCPP_INFO(node->get_logger(),"==================== exec ====================");

}

void CTaskNav2::_SendStopReq(int reason)
{
    std_msgs::msg::Int32 controlInfo;
    controlInfo.data = 0;
    g_clsData._pub_nav_task_control->publish(controlInfo);

    RCLCPP_INFO(node->get_logger(),"==================== exec ====================");
}

void CTaskNav2::_PubNavLimit(NavTaskLimitIndex *taskLimit)
{

    ubt_interface::msg::NavTaskLimit taskLimitMsg;
    taskLimitMsg.task_id = taskLimit->taskId;
    taskLimitMsg.nav_type = g_clsData.navType;
    taskLimitMsg.wait_line_index = taskLimit->waitLineIndex;
    taskLimitMsg.wait_point_id = taskLimit->waitPointId;
    taskLimitMsg.cmd = taskLimit->cmd;
    g_clsData._pub_nav_task_limit->publish(taskLimitMsg);
    if(taskLimit->cmd == 0)
    {
        ubt_state::ulogger->info("交管暂停--\n_taskId= {} _state={0:d} waitLineIndex={0:d} waitPointId: {0:d} ", _taskId.c_str(), _state,taskLimit->waitLineIndex,taskLimit->waitPointId);
    }

    RCLCPP_INFO_STREAM(node->get_logger(),"taskId= " <<
        taskLimit->taskId << " waitLineIndex: " << taskLimit->waitLineIndex <<
        " waitPointId: " << taskLimit->waitPointId << " cmd: " << taskLimit->cmd);
}

void CTaskNav2::OnSiteChanged(int pathIndex)
{
    // ubt_state::ulogger->info("-------------- 获取到点 --------pathIndex{0:d}------",pathIndex);
    //_point_status只有_pLineInfos->size()-1的数据
    RCLCPP_INFO(node->get_logger(),"[CTaskNav2] OnSiteChanged start");
    RCLCPP_INFO_STREAM(node->get_logger(),"pathIndex "<< pathIndex <<" "<<_pLineInfos->size());

    //数组越界检查
    if((int)pathIndex > (int)_pLineInfos->size())
    {
        RCLCPP_ERROR(node->get_logger(),"out_off_index pathIndex= %d,_pLineInfos->size= %d",pathIndex,int(_pLineInfos->size()));
        return;
    }

    //补发机制---所有上报类型mag slam fusion
    for(int i = 0; i<pathIndex; i++)
    {
        if(_point_status[i] == 0)
        {
            ubt_state::ulogger->info("补发的点-- index= {0:d},points_size= {1:d}", i,_pLineInfos->size());
            _OnArrive(i);
            _point_status[i] = 1;
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
    }

    //处理当前点
    //1.起点
    if(pathIndex  == -1)
    {
        ubt_state::ulogger->info("起步-- index= {0:d},points_size= {1:d}", pathIndex,_pLineInfos->size());
        _OnArrive(-1);
        return;
    }
    //2.其他类型点
    if( _point_status[pathIndex] == 0 )
    {
        // 如果到达最后一个地图点
        if(pathIndex == _pLineInfos->size())
        {
            ubt_state::ulogger->info("到达目标点-- index= {0:d},points_size= {1:d}", pathIndex,_pLineInfos->size());
            _OnArrive(-2);
            _point_status[pathIndex] = 1;
        }
        else
        {
            ubt_state::ulogger->info("发出的点-- index= {0:d},points_size= {1:d}", pathIndex,_pLineInfos->size());
            _OnArrive(pathIndex);
            _point_status[pathIndex] = 1;  
        }
    }
    // ubt_state::ulogger->info("-------------- 获取到点 --------------\n");
    
}

void CTaskNav2::SetStateWrap(ubt_task_state newState, int reason, std::string reasonStr)
{
    _SetState(newState, reason, reasonStr);
}

void CTaskNav2::_OnLocationCb()
{
    
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    geometry_msgs::msg::Pose2D *pCurPos = &pStatus->curPose;
    //膨胀长度，不带货架时是机器人长，带货架时为膨胀后的长度
    double expLength = pStatus->expLength;
    Point *pPtNext = NULL, *pPtrCur = NULL;
    do
    {
        if ((_pathIndex < -3) || (_pathIndex == -2) ||
                (_pathIndex >= (int)_pLineInfos->size()))
        {
            break;
        }
        float stopDis, slowDis, warnDis, sideDis;
        if (_pathIndex == -1 || _pathIndex == -3)
        {
            pPtrCur = &(*_pLineInfos)[0].points[0];
            pPtNext = &(*_pLineInfos)[0].points[1];
            stopDis = g_clsData._obstacle_stop_dis;
            slowDis = g_clsData._obstacle_slow_dis;
            warnDis = g_clsData._obstacle_warn_dis;
            sideDis = g_clsData._obstacle_side_dis;
        }
        else
        {
            pPtrCur = &(*_pLineInfos)[_pathIndex].points[0];
            pPtNext = &(*_pLineInfos)[_pathIndex].points[1];
            /*
            stopDis = (*_pLineInfos)[_pathIndex].stop;
            slowDis = (*_pLineInfos)[_pathIndex].slow;
            warnDis = (*_pLineInfos)[_pathIndex].warning;
            sideDis = (*_pLineInfos)[_pathIndex].side;
            */

            stopDis = g_clsData._obstacle_stop_dis;
            slowDis = g_clsData._obstacle_slow_dis;
            warnDis = g_clsData._obstacle_warn_dis;
            sideDis = g_clsData._obstacle_side_dis;
        }

        static bool needApplyLineProp = true;

        if ((pPtNext->avoidDis < 0 && pPtrCur->avoidDis < 0) ||
                ((pPtNext->avoidDis >= stopDis) && (pPtrCur->avoidDis >= stopDis)))
        {
            if(needApplyLineProp)
            {
                needApplyLineProp = false;
                // _ApplyLineProp(_pathIndex);
            }

            break;
        }

        needApplyLineProp = true;

        double xOffCur = fabs(pCurPos->x - pPtrCur->x), yOffCur = fabs(pCurPos->y - pPtrCur->y);
        double disCur = sqrt(xOffCur * xOffCur + yOffCur * yOffCur);

        double xOffNext = fabs(pCurPos->x - pPtNext->x), yOffNext = fabs(pCurPos->y - pPtNext->y);
        double disNext = sqrt(xOffNext * xOffNext + yOffNext * yOffNext);

        //机器人在哪个位置，-1：默认值，0：机器人在当前点，
        //1：机器人在下一个点内，2：机器人在当前点到下一个点的路上
        int robotPoseStatus = -1;
        //发布的避障距离是哪个位置，-1：默认值，0：发布的当前点的避障距离，
        //1：发布下一个点的避障距离，2：发布路上的避障距离
        static int pubAvoidStatus = -1;

        //保存上一个路径序号
        static int lastIndex = -3;
        //如果走过了一个二维码，则重置发布避障距离状态参数
        if(lastIndex != _pathIndex)
        {
            pubAvoidStatus = -1;
            lastIndex = _pathIndex;
        }

        //先判断是否在下一个点内
        if(disNext - expLength <= stopDis)
        {
            robotPoseStatus = 1;
        }
        //机器人在当前点内时，用当前点的避障距离
        else if(disCur <= stopDis)
        {
            robotPoseStatus = 0;
        }
        //否则，认为机器人在路上
        else
        {
            robotPoseStatus = 2;
        }

        //如果发布的避障距离是所在路段的，则不用在设
        if(robotPoseStatus == pubAvoidStatus)
        {
            break;
        }

        //设置避障参数
        RCLCPP_INFO(node->get_logger(),"before send obstacle_dis _taskId= %s "
                "pathIndex=%d dis=(%f,%f,%f,%f) curPose=(%lf,%lf) robotPoseStatus=%d, "
                "pubAvoidStatus=%d, expLength=%f, disCur=%f, disNext=%f",
                _taskId.c_str(), _pathIndex, stopDis, slowDis, warnDis, sideDis, pCurPos->x, pCurPos->y,
                robotPoseStatus, pubAvoidStatus, expLength, disCur, disNext);

        RCLCPP_INFO(node->get_logger(),"before pPtrCur->avoidDis:%f, pPtNext->avoidDis:%f",
                pPtrCur->avoidDis, pPtNext->avoidDis);

        //机器人在当前点内时，发布的当前点的避障距离
        if(0 == robotPoseStatus)
        {
            pubAvoidStatus = 0;
            if(pPtrCur->avoidDis < 0) break;

            if (stopDis > pPtrCur->avoidDis)
            {
                stopDis = pPtrCur->avoidDis;
            }
            if (sideDis > pPtrCur->avoidDis)
            {
                //站点避障距离需影响侧面
                sideDis = pPtrCur->avoidDis;
            }

            _PubObstacleDis(stopDis, slowDis, warnDis, sideDis); // not use
        }
        //发布下一个点的避障距离
        else if(1 == robotPoseStatus)
        {
            pubAvoidStatus = 1;
            if(pPtNext->avoidDis < 0) break;

            if (stopDis > pPtNext->avoidDis)
            {
                stopDis = pPtNext->avoidDis;
            }
            if (sideDis > pPtNext->avoidDis)
            {
                //站点避障距离需影响侧面
                sideDis = pPtNext->avoidDis;
            }

            _PubObstacleDis(stopDis, slowDis, warnDis, sideDis);  // not use
        }
        //发布路上的避障距离
        else if(2 == robotPoseStatus)
        {
            pubAvoidStatus = 2;
            // _ApplyLineProp(_pathIndex);
        }

        //设置避障参数
        RCLCPP_INFO(node->get_logger(),"after send obstacle_dis _taskId= %s "
                "pathIndex=%d dis=(%f,%f,%f,%f) curPose=(%lf,%lf) robotPoseStatus=%d, "
                "pubAvoidStatus=%d, expLength=%f, disCur=%f, disNext=%f",
                _taskId.c_str(), _pathIndex, stopDis, slowDis, warnDis, sideDis, pCurPos->x, pCurPos->y,
                robotPoseStatus, pubAvoidStatus, expLength, disCur, disNext);
    }
    while (false);
    
}
//避障状态
void CTaskNav2::_OnAvoidStatusCb()
{
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    _avoidHwStatus = pStatus->avoidHwStatus;
    if (_pathIndex == -1)
    {
        if (_prePathAvoidHwMask & avoid_hw_mask_rgbd)
        {
            _avoidHwStatus = pStatus->laserAvoidStatus;
        }
    }
}

void CTaskNav2::_OnArrive(int pathIndex)
{
    RCLCPP_INFO(node->get_logger(),"[CTaskNav2] _OnArrive start pathIndex %d, _pathIndex %d, _pLineInfos: %d", pathIndex, _pathIndex, _pLineInfos->size());
    if ((pathIndex >= (int)_pLineInfos->size()) ||
            (pathIndex < - 2) ||
            (pathIndex == _pathIndex) ||
            _pLineInfos->empty())
    {
        return;
    }
    _ApplyLineProp(pathIndex);
    _pathIndex = pathIndex;
    StateStatus *pStatus = _pTaskCb->GetStateStatus();

    // u1000 or u600 second round recharge not reporting real_route_info
    if ((_agv_type.substr(0, 5) == "u1000" || _agv_type.substr(0, 4) == "u600") && _chargeExLoop > 0)
    {
        return;
    }

    // std_msgs::msg::Int16MultiArray pathMsg;
    std_msgs::msg::Int32MultiArray pathMsg;
    Line *pLine;
    Point *pPt1, *pPt2;
    unsigned short id1, id2;
    if (_pathIndex >= 0)
    {
        pLine = &(*_pLineInfos)[_pathIndex];
        pPt1 = &pLine->points[0];//0 is first point
        pPt2 = &pLine->points[1];//1 is last point
        _lastPtId = pPt1->id;
        _nextPtId = pPt2->id;
        
        if (_agv_type == "u1000_v3" || _agv_type == "t1000_v2" || _agv_type == "u600")
        {
            
        }
        else
        {
            // aeb::SetStopDistance ssd_srv;

            rclcpp::Client<ubt_interface::srv::SetStopDistance>::SharedPtr  _set_stop_distance_client
                = node->create_client<ubt_interface::srv::SetStopDistance>("/aeb/set_stop_distance");

            auto request = std::make_shared<ubt_interface::srv::SetStopDistance::Request>();
            request->distance = pPt1->avoidDis;
            while (!_set_stop_distance_client->wait_for_service(1s)) { 
              RCLCPP_INFO(node->get_logger(),"/aeb/set_stop_distance service not available, waiting again...");
            }

            auto result = _set_stop_distance_client->async_send_request(request); 
            if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
            {
                RCLCPP_INFO(node->get_logger(),"请求正常处理,响应结果: is success: %d, msg: %s",result.get()->success, result.get()->message.c_str());
            }
            else
            {
                RCLCPP_ERROR(node->get_logger(),"请求处理失败....");
                return;
            }
        }
       
    }
    else
    {
        if (-1 == _pathIndex)
        {
            pLine = &(*_pLineInfos)[0];
            pPt1 = &pLine->points[0];
            _lastPtId = -1;
            _nextPtId = pPt1->id;
        }
        else//=-2
        {
            pLine = &(*_pLineInfos)[_pLineInfos->size() - 1];
            pPt2 = &pLine->points[1];
            _lastPtId = pPt2->id;
            _last_task_id = _lastPtId;//每次到终点就记录
            _nextPtId = -2;
        }
    }

    pathMsg.data.push_back(_lastPtId < 0 ? 0 : _lastPtId);
    pathMsg.data.push_back(_nextPtId < 0 ? 0 : _nextPtId);
    pathMsg.data.push_back(_pathIndex);
    g_clsData._pub_real_route_info->publish(pathMsg);

    RCLCPP_INFO(node->get_logger(),"_taskId= %s _pathIndex=%d _lastPtId=%d -> _nextPtId=%d curPos=(%lf,%lf,%lf)",
            _taskId.c_str(), _pathIndex, _lastPtId, _nextPtId,
            pStatus->curPose.x, pStatus->curPose.y, pStatus->curPose.theta);
    
    
    ubt_state::ulogger->info("-----111111-_last_task_id= {0:d}--start- ",_last_task_id);
    if(_pathIndex == -1 && _agv_type == "t1000_v2")//T1000都需要补发0
    {
        ubt_state::ulogger->info("-----2222-_last_task_id= {0:d}--start- ",_last_task_id);
        _wait_rfid = 0;
        // std_msgs::msg::Int16MultiArray end_pathMsg;
        std_msgs::msg::Int32MultiArray end_pathMsg;
        pLine = &(*_pLineInfos)[0];
        pPt1 = &pLine->points[0];//0 is first point
        pPt2 = &pLine->points[1];//1 is last point
        _lastPtId = pPt1->id;
        _nextPtId = pPt2->id;
        std::string  now_rfid = pPt1->rfid;
        if(_last_task_id == _lastPtId)//如果当前就在当前点上起步
        {
            rclcpp::sleep_for(std::chrono::milliseconds(100));
            ubt_state::ulogger->info("-----补发的点(当前点起步)-now_rfid= {}--start- ",now_rfid);
            // ubt_interface::msg::Rfid msg;
            ubt_interface::msg::Rfid::SharedPtr msg;
            msg->id = now_rfid;
            msg->is_induction = 1;
            current_rfid = "";//赋值为空才能补发该点，因为OnRecvNavRfidWrap函数中有防止重复上报逻辑
            OnRecvNavRfidWrap(msg);
            ubt_state::ulogger->info("-----补发的点(当前点起步)-now_rfid= {}--end- ",now_rfid);
            
        }
    }
    
}

}



